#include <hidef.h>
#include "derivative.h"
#include "i2c.h"

void i2c_init (void)
{
	/* Initialize I2C */
	IBFD = 0x1F;
	IBAD = 0x17;

}

int i2c_getAngle (void)
{
	int heading;
	IBCR_IBEN = 1;
	while (IBSR_IBB) continue;
	IBCR_MS_SL = 1;
	IBCR_TX_RX = 1;           		    //Grab bus, send start bit
	
	IBDR = 0xC0;						          //Send ADDR of compass module
	while (!IBSR_IBIF) continue;	  //Wait for Transfer to complete
	IBSR_IBIF = 1;
	//while (!IBSR_RXAK) continue;
	
	IBDR = 0x02;          						//Send base register number
	while (!IBSR_IBIF) continue;	  //Wait for Transfer to complete
	IBSR_IBIF = 1;
	//while (IBSR_RXAK) continue;
	
	IBCR_RSTA=1;					          	//Send repeated start bit
	IBDR = 0xC1;		          	      //Send ADDR with receive direction bit
	while (!IBSR_IBIF) continue;	  //Wait for Transfer to complete
	IBSR_IBIF = 1;
	//while (IBSR_RXAK) continue;
	
	IBCR_TX_RX = 0;						        //Toggle to Receive mode	
	IBCR_TXAK = 0;
	heading=IBDR;				            	//Dummy Read
	while (!IBSR_IBIF) continue;	  //Wait for Transfer to complete
	IBSR_IBIF = 1;
	heading=IBDR<<8;				            	//Dummy Read
	while (!IBSR_IBIF) continue;	  //Wait for Transfer to complete
	IBSR_IBIF = 1;
	IBCR_TXAK=1;
  heading=heading+IBDR;				          	//Read High Byte of Angle
  while (!IBSR_IBIF) continue;	  //Wait for Transfer to complete
	IBSR_IBIF = 1;
	
	//heading[1] = IBDR;         					//Read Low Byte of Angle
	//while (!IBSR_IBIF) continue;	  //Wait for Transfer to complete
	//IBSR_IBIF = 1;
	IBCR_MS_SL=0; 				  	        //Toggle Master off, send stop bit
	IBCR_IBEN=0;
	return(heading);
}
